
	#include "DigitalFilters.h"
	
/******************************************************************************
*   					LOCAL VARIABLE DEFINITIONS
******************************************************************************/
	_iq *FilterInputsPtr,*FilterOutputsPtr;
	_iq *FilterCoefficientsPtr;
	
	#if FILTER_TYPE == FIR
	// Xvalues -  is a buffer, where all previous data is storage from each channel of the filter
	// Xvavlues = TAP FILTER x FILTER CHANNELS
		_iq Xvalues[TAP_FILTER*FILTER_CHANNELS];
		_iq *XvaluesPtr;
	#endif
	#if FILTER_TYPE == IIR
		_iq Xvalues[(FILTER_ORDER+1)*FILTER_CHANNELS];
		_iq Yvalues[(FILTER_ORDER+1)*FILTER_CHANNELS];
		_iq *XvaluesPtr,*YvaluesPtr;
	#endif
	
/******************************************************************************
*   					GLOBAL VARIABLE DEFINITIONS
******************************************************************************/
		// FilterInputs - variable were new data is introduce it to the Filter
	_iq FilterInputs[FILTER_CHANNELS];		
		// FilterOutputs - variable were data filtered is store.
	_iq FilterOutputs[FILTER_CHANNELS];
	

/******************************************************************************
*   					LOCAL FUNCTIONS DEFINITIONS
******************************************************************************/
	void VfnCoefficientsMultiplication(_iq* Buffer,_iq* coefficients);
	void VfnIIRCoefficientsMultiplication(_iq* BufferA,_iq* BufferB,_iq* CoefficientsA,_iq* CoefficientsB);

/******************************************************************************
*   						GLOBAL FUNCTIONS 
******************************************************************************/
/*****************************************************************************
 * Function: 	VfnFIRFilterSetup
 * Description: Setup data pointers.
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/
	#if FILTER_TYPE == FIR
	void VfnFIRFilterSetup(void)
	{
		FilterInputsPtr = &FilterInputs[0];
		FilterOutputsPtr = &FilterOutputs[0];
		XvaluesPtr = &Xvalues[0];
	}

/*****************************************************************************
 * Function: 	VfnFIRFilter
 * Description: FIR algorithm.
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/
	void VfnFIRFilter (void)
	{
		// First all values on XValues buffer are logic swap
		volatile unsigned char Nvalue,Nchannel;
		Nvalue = 0;
		Nchannel = 0;
		while(Nchannel != (FILTER_CHANNELS))
		{
			Nvalue=TAP_FILTER-1;
			while(Nvalue != (0))
			{
				Xvalues[(Nvalue)+(Nchannel*(TAP_FILTER))] = Xvalues[(Nvalue+(Nchannel*(TAP_FILTER)))-1];
				Nvalue--;
			}
			Nchannel++;
		}
			
		Nchannel=0;
		while(Nchannel != FILTER_CHANNELS)
		{
			//Pass the Inpunt values to the Filter buffer "For each channel"
			*((XvaluesPtr)+ (Nchannel * TAP_FILTER)) = *((FilterInputsPtr)+Nchannel);
			Nchannel ++;
		}
		VfnCoefficientsMultiplication(&Xvalues[0],&FIRCoefficients[0]);
	}
	#endif

/*****************************************************************************
 * Function: 	VfnIIRFilterSetup
 * Description: Setup data pointers.
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/	
	#if FILTER_TYPE == IIR
	void VfnIIRFilterSetup(void)
	{
		FilterInputsPtr = &FilterInputs[0];
		FilterOutputsPtr = &FilterOutputs[0];
		XvaluesPtr = &Xvalues[0];
		YvaluesPtr = &Yvalues[0];
	}

/*****************************************************************************
 * Function: 	VfnIIRFilter
 * Description: IIR algorithm.
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/
	void VfnIIRFilter (void)
	{
		volatile unsigned char Nvalue,Nchannel;
		Nchannel = FILTER_CHANNELS;	
		
		Nchannel = 0;
		//Shift the old Samples on each filter buffer
		while(Nchannel != (FILTER_CHANNELS))
		{
			Nvalue=FILTER_ORDER;
			while(Nvalue != 0)
			{
				Xvalues[(Nvalue)+(Nchannel*(FILTER_ORDER+1))] = Xvalues[(Nvalue)+(Nchannel*(FILTER_ORDER+1))-1];
				Yvalues[(Nvalue)+(Nchannel*(FILTER_ORDER+1))] = Yvalues[(Nvalue)+(Nchannel*(FILTER_ORDER+1))-1];
				Nvalue--;
			}
			Nchannel++;
		}
		//Pass the Inpunt value only in X buffer
		Nchannel = 0;
		while(Nchannel != FILTER_CHANNELS)
		{
			*((XvaluesPtr)+ (Nchannel * (FILTER_ORDER+1))) = *((FilterInputsPtr)+Nchannel);
			Nchannel ++;
		}
		VfnIIRCoefficientsMultiplication(XvaluesPtr,YvaluesPtr,&ACoefficients[0],&BCoefficients[0]);
		Nchannel = 0;
		while(Nchannel != FILTER_CHANNELS)
		{
			*((YvaluesPtr)+ (Nchannel * (FILTER_ORDER+1))) = *((FilterOutputsPtr)+Nchannel);
			Nchannel ++;
		}
	}
	#endif

/******************************************************************************
*   						LOCAL FUNCTIONS
******************************************************************************/	
/*****************************************************************************
 * Function: 	VfnCoefficientsMultiplication 
 * Description: Coefficients multiplication, FIR type. (buffer[0] * Coef0)
 * Parameters:	Buffer pointer and Coefficients pointer.
 * Caveats:		
 * Return:		(Actualize) Filter Output.
 *****************************************************************************/
	void VfnCoefficientsMultiplication(_iq* Buffer,_iq* Coefficients)
	{
		volatile unsigned char Nvalue,Nchannel;
		Nvalue = 0;
		Nchannel = 0;
		while(Nchannel != (FILTER_CHANNELS))
		{
			FilterOutputs[Nchannel]=_IQ(0);
			Nchannel++;
		}
		Nchannel=0;
		while(Nchannel != (FILTER_CHANNELS))
		{
			Nvalue=0;
			while(Nvalue != (TAP_FILTER))
			{
				*((FilterOutputsPtr) + Nchannel) += _IQmpy(*(Buffer+(Nvalue)+(Nchannel*(TAP_FILTER))),*(Coefficients+Nvalue));
				Nvalue++;
			}
			Nchannel++;
		}
	}

/*****************************************************************************
 * Function: 	VfnIIRCoefficientsMultiplication
 * Description: Coefficients multiplication. (bufferA[0] * CoefA0) + (bufferB[0] * CoefB0)
 * Parameters:	Buffer pointer and Coefficients pointer.
 * Caveats:		
 * Return:		(Actualize) Filter Output.
 *****************************************************************************/
	void VfnIIRCoefficientsMultiplication(_iq* BufferA,_iq* BufferB,_iq* CoefficientsA,_iq* CoefficientsB)
	{
		volatile unsigned char Nvalue,Nchannel;
		Nchannel=0;
		while(Nchannel != (FILTER_CHANNELS))
		{
			*((FilterOutputsPtr) + Nchannel) = _IQmpy(*(BufferA+(Nchannel*(FILTER_ORDER+1))),*CoefficientsA);
			Nchannel++;
		}
			
		Nchannel=0;
		while(Nchannel != (FILTER_CHANNELS))
		{
			Nvalue=1;
			while(Nvalue != (FILTER_ORDER)+1)
			{
				*((FilterOutputsPtr) + Nchannel) += _IQmpy(*(BufferA+(Nvalue)+(Nchannel*(FILTER_ORDER+1))),*(CoefficientsA+Nvalue))- _IQmpy(*(BufferB+(Nvalue)+(Nchannel*(FILTER_ORDER+1))),*(CoefficientsB+Nvalue));
				Nvalue++;
			}
			Nchannel++;
		}
	}
